#include "motor.h"

float voltagePowerSupply = 12.2; // 母线电压
float dutyA = 0; // A相占空比
float dutyB = 0; // B相占空比
float dutyC = 0; // C相占空比

volatile float electricalAngle; // 电角度

volatile float estimateAngle; // 开环虚拟电角度
const static float voltageLimit = 0.5f; // 开环电压限制

volatile float zeroElectricAngleOffset = 0.12; // 传感器0电角度
const static int polePairs = 7; // 极对数
const static int countDirection = 1; // 电机方向

volatile float current_target = 0.f; // 当前电流

PID_LocTypeDef current_pid_q; // q轴电流PID
PID_LocTypeDef current_pid_d; // d轴电流PID

lowpass_filter_type LPF_current_d = {0}; // d轴电流低通滤波
lowpass_filter_type LPF_current_q = {0}; // q轴电流低通滤波

volatile float currentD, currentQ; // d,q轴电压设定值
volatile float voltageA, voltageB, voltageC; // 根据母线电压和占空比计算的ABC相电压

volatile ControlMode_t CURR_MODE = NOT_STARTED; // 运行模式

// 三相桥初始化,参数设置
void eg2133_Init(void)
{
  driver_ConfigTimerForPwm();

  current_pid_q.kp = 5;
  current_pid_q.ki = 0.5;
  current_pid_q.kd = 0;
  current_pid_q.limit = 3;
  current_pid_q.ka = 1;

//  2.0.3,0
  current_pid_d.kp = 5;
  current_pid_d.ki = 0.5;
  current_pid_d.kd = 0;
  current_pid_d.limit = 3;
  current_pid_d.ka = 1;

  LPF_current_d.sample_freq = 16000,
  LPF_current_d.bandwidth = 3000,

  LPF_current_q.sample_freq = 16000;
  LPF_current_q.bandwidth = 3000;

  lowpass_filter_init(&LPF_current_d);
  lowpass_filter_init(&LPF_current_q);
}

// 设置ABC三相电压
void SetVoltage(float _voltageA, float _voltageB, float _voltageC)
{
  _voltageA = CONSTRAINT(_voltageA, 0, voltagePowerSupply);
  _voltageB = CONSTRAINT(_voltageB, 0, voltagePowerSupply);
  _voltageC = CONSTRAINT(_voltageC, 0, voltagePowerSupply);

  dutyA = _voltageA / voltagePowerSupply;
  dutyB = _voltageB / voltagePowerSupply;
  dutyC = _voltageC / voltagePowerSupply;

  driver_SetPwmDutyByRegister(dutyA, dutyB, dutyC);
}

// 开环运行测试
float VelocityOpenLoopTick(float _target)
{
  float deltaT = 0.0000625; // 开环运行时间间隔

  estimateAngle = Normalize(estimateAngle + _target * deltaT);

  float voltageQ = voltageLimit;
  SetPhaseVoltage(voltageQ, 0, Normalize(estimateAngle) * (float) polePairs);

  return voltageQ;
}

// 电角度获取
float GetElectricalAngle(void)
{
  // If no sensor linked return previous value (for open-loop case)
  return Normalize((float)(countDirection * polePairs)
                   * encoder_GetLapAngle() - zeroElectricAngleOffset);
}

// FOC运算函数
void motor_FocOutputTick(void)
{
  encoder_Update();
  electricalAngle = GetElectricalAngle();
//  printf("%.2f\r\n", electricalAngle);

  DqCurrent_t current = getFOCCurrents(electricalAngle);

  currentD = lowpass_filtering(&LPF_current_d, current.d);
  currentQ = lowpass_filtering(&LPF_current_q, current.q);

  switch(CURR_MODE)
  {
  case OPENLOOP:
  {
    VelocityOpenLoopTick(5);
    break;
  }
  case VELOCITY:
  case ANGLE:
  case CURRENT:
  {
    float pid_Uq = PID_Loc(current_target, currentQ, &current_pid_q);
    float pid_Ud = PID_Loc(0, currentD, &current_pid_d);
//    printf("%.2f,%.2f,%.2f\r\n", currentQ, currentD, current.q);
    SetPhaseVoltage(pid_Uq, pid_Ud, electricalAngle);
    break;
  }
  default:
    break;
  }
}

// 0电角度对齐
void AlignSensor(void)
{
  delay_ms(500);
  // Set angle -90(270 = 3PI/2) degrees
  SetPhaseVoltage(3, 0, _3PI_2); // 强拖电机到3PI/2

  delay_ms(1500);
  encoder_Update();
  zeroElectricAngleOffset = GetElectricalAngle(); // 获得0电角度
  SetPhaseVoltage(0, 0, 0);
  delay_ms(200);

  printf("zeroElectricAngleOffset = %.2f\r\n", zeroElectricAngleOffset);
}

// SVPWM电压计算
void SetPhaseVoltage(float _voltageQ, float _voltageD, float _angleElectrical)
{
  float uOut;

  if(_voltageD != 0)
  {
    float atanResult;
    arm_atan2_f32(_voltageQ, _voltageD, &atanResult);
    uOut = SQRT(_voltageD * _voltageD + _voltageQ * _voltageQ) / voltagePowerSupply;
    _angleElectrical = Normalize(_angleElectrical + atanResult);
  }
  else
  {
    uOut = _voltageQ / voltagePowerSupply;
    _angleElectrical = Normalize(_angleElectrical + _PI_2);
  }
  uint8_t sec = (int)(floor(_angleElectrical / _PI_3)) + 1;
  float t1 = _SQRT3
             * arm_sin_f32((float)(sec) * _PI_3 - _angleElectrical)
             * uOut;
  float t2 = _SQRT3
             * arm_sin_f32(_angleElectrical - ((float)(sec) - 1.0f) * _PI_3)
             * uOut;
  float t0 = 1 - t1 - t2;

  float tA, tB, tC;
  switch(sec)
  {
  case 1:
    tA = t1 + t2 + t0 / 2;
    tB = t2 + t0 / 2;
    tC = t0 / 2;
    break;
  case 2:
    tA = t1 + t0 / 2;
    tB = t1 + t2 + t0 / 2;
    tC = t0 / 2;
    break;
  case 3:
    tA = t0 / 2;
    tB = t1 + t2 + t0 / 2;
    tC = t2 + t0 / 2;
    break;
  case 4:
    tA = t0 / 2;
    tB = t1 + t0 / 2;
    tC = t1 + t2 + t0 / 2;
    break;
  case 5:
    tA = t2 + t0 / 2;
    tB = t0 / 2;
    tC = t1 + t2 + t0 / 2;
    break;
  case 6:
    tA = t1 + t2 + t0 / 2;
    tB = t0 / 2;
    tC = t1 + t0 / 2;
    break;
  default:
    tA = 0;
    tB = 0;
    tC = 0;
  }

  // calculate the phase voltages and center
  voltageA = tA * voltagePowerSupply;
  voltageB = tB * voltagePowerSupply;
  voltageC = tC * voltagePowerSupply;

  SetVoltage(voltageA, voltageB, voltageC);
}
